#include "drv_OpticalCar.h"


// Range Status
#define TOF_RANGE_VALID         0
#define TOF_RANGE_PHASE_FAIL    4


// Avoiding Parameter
#define TURN_DIS  135   // distance to turn around
#define BACK_DIS  50    // distance to go backward


static void Car_AvoidPolicy(uint16_t distance1, uint16_t distance2) {
    int _obstacle_right = TURN_DIS > distance1;  // has obstacle in fornt of right ?
    int _obstacle_left = TURN_DIS > distance2;   // or in front left ?
    
    // ===== Special Conditions =====
    // Go back if obstacle is too close
    if (BACK_DIS > distance1 || BACK_DIS > distance2) {
        Car_Backward();
        return;
    }
    
    // Obstecle on left or on both sides
   if (_obstacle_left) {
        Car_TurnRight();
        return;
    }
    
    // Obstacle on right only 
    else if (_obstacle_right) {
        Car_TurnLeft();
        return;
    }
    
    // ===== Normal Conditions =====
    Car_Forward();
}


void OpticalCar_main(void) {
    VL53L0X_RangingMeasurementData_t _meas1, _meas2;
    
    while (1) {
        Car_TOF_Measure(&_meas1, &_meas2);
        printf("\x7f");  // clear screen
        printf("  %4i    %4i  ", _meas2.RangeMilliMeter, _meas1.RangeMilliMeter);
        
        if (   TOF_RANGE_VALID == _meas1.RangeStatus 
            && TOF_RANGE_VALID == _meas2.RangeStatus) 
        {  
            /* No error, pass */
        }
        else {
            // Print status
            printf(" %i %s  ", _meas1.RangeStatus,
                (TOF_RANGE_PHASE_FAIL == _meas2.RangeStatus) ? " OL" : "Err");
            printf("  %i %s ", _meas2.RangeStatus,
                (TOF_RANGE_PHASE_FAIL == _meas1.RangeStatus) ? " OL" : "Err");
            
            // Error handle
            if (   TOF_RANGE_PHASE_FAIL != _meas1.RangeStatus
                || TOF_RANGE_PHASE_FAIL == _meas2.RangeStatus) {
                    Car_Stop();
            }
        }
        
        Car_AvoidPolicy(_meas1.RangeMilliMeter, _meas2.RangeMilliMeter);
    }
}

